
//#include <SoftwareSerial.h>

float convertCharArrayToFloat(char* gpsBufferInput, int startByte, int endByte);
void printTestData();
void readSensorData();
void checkBuffer();
void findCommas();
void processBuffer();
void clearBuffer();
void initGps();
void setup();
void loop();
int rxGpsPin = 0;                    // RX PIN 
int txGpsPin = 1;                    // TX PIN
//int rxBlueToothPin = 3;                    // RX GPS PIN 
//int txBlueToothPin = 2;                    // TX GPS PIN

int byteGPS=-1;
int bufferIndex = 300;
char gpsDataBuffer[300] = "";
int indices[13];
char comandoGPR[7] = "$GPRMC";


//SoftwareSerial mySerial = SoftwareSerial(rxBlueToothPin, txBlueToothPin);

boolean validData = false;
float velocityMetersPerSecond = 0.0f;
int locationHeading = 0;

float convertCharArrayToFloat(char* gpsBufferInput, int startByte, int endByte){
	
	float returnValue = 0.0f;
	char gpsDataPart[50] = "";
	int partSize = 0;
	for(int index = startByte; index < endByte; index++, partSize++){
		gpsDataPart[partSize] = gpsBufferInput[index];
	}
	
	returnValue = strtod(gpsDataPart, NULL);
	
	return returnValue;
}

void printTestData(){
	
	Serial.print("Status (A=OK,V=KO): ");
	if(validData){
		Serial.print("true");
	}else{
		Serial.print("false");
	}
	Serial.print(" - Velocity in tenths of Meters Per Second: ");
	Serial.print(velocityMetersPerSecond * 10, DEC);
	Serial.print(" - Heading in degrees: ");
	Serial.print(locationHeading);  
	Serial.println("");
}


int cont=0;
int isRmcMessage=0;
int conta=0;
bool doneReading = false;



void readSensorData(){
	byteGPS = Serial.read();         // Read a byte of the serial gps port
	if (byteGPS != -1) {           // See if the port is empty yet
		gpsDataBuffer[bufferIndex]=byteGPS;        // If there is serial port data, it is put in the buffer
		bufferIndex++;     
		if (byteGPS==13){            // If the received byte is = to 13, end of transmission
			checkBuffer();
			//printTestData();
			clearBuffer();
		}
	}
}

void checkBuffer(){
	bool bufferIsRmcMessage = true;
	for (int i=1;i<7 && bufferIsRmcMessage;i++){     // Verifies if the received command starts with $GPR
		if (gpsDataBuffer[i]!=comandoGPR[i-1]){
			bufferIsRmcMessage = false;
			Serial.print(gpsDataBuffer[i-1]);
			Serial.print(" != ");
			Serial.println(comandoGPR[i-1]);
		}else{
			
			Serial.print(gpsDataBuffer[i-1]);
			Serial.print(" == ");
			Serial.println(comandoGPR[i-1]);
		}
	}
	if(bufferIsRmcMessage){
		Serial.print("isRMC");
		findCommas();
		processBuffer();
	}
}

void findCommas(){
	for(int commasFound = 0, index = 0; commasFound < 13 && index < 300; index++){
		if(gpsDataBuffer[index] == ',' || gpsDataBuffer[index] == '*'){
			indices[commasFound] = index;
			Serial.print(commasFound);
			Serial.print(gpsDataBuffer[index]);
			commasFound++;
		}
	}
	Serial.print("past commas");
	
}

void processBuffer(){
	Serial.print("processing");
	for (int i=0;i<12;i++){
		switch(i){
			case 0 :
				//Serial.print("Time in UTC (HhMmSs): ");
				break;
			case 1 :
				validData = gpsDataBuffer[indices[i]+1] == 'A';
				break;
			case 2 :
				//Serial.print(" - Latitude: ");
				break;
			case 3 :
				//Serial.print(" - Direction (N/S): ");
				break;
			case 4 :
				//Serial.print(" - Longitude: ");
				break;
			case 5 :
				//Serial.print(" - Direction (E/W): ");
				break;
			case 6 :
				velocityMetersPerSecond = convertCharArrayToFloat(gpsDataBuffer, indices[i]+1, indices[i+1]) * .51444f;
				break;
			case 7 :
				locationHeading = convertCharArrayToFloat(gpsDataBuffer, indices[i]+1, indices[i+1]);
				break;
			case 8 :
				//Serial.print(" - Date UTC (DdMmAa): ");
				break;
			case 9 :
				//Serial.print(" - Magnetic degrees: ");
				break;
			case 10 :
				//Serial.print(" - (E/W): ");
				break;
			case 11 :
				//Serial.print(" - Mode: ");
				break;
			case 12 :
				//Serial.print(" - Checksum: ");
				break;
		}
	}
	
}

void clearBuffer(){
	for (int i=0;i<300;i++){       // Initialize a buffer for received data
		gpsDataBuffer[i]=' ';
	}   
	bufferIndex = 0;
}

void initGps() {
	pinMode(rxGpsPin, INPUT);
	pinMode(txGpsPin, OUTPUT);
	//pinMode(rxBlueToothPin, INPUT);
	//pinMode(txBlueToothPin, OUTPUT);
	
	Serial.begin(4800);
	//mySerial.begin(4800);
	clearBuffer();
}

void setup(){
	initGps();
}

void loop(){
	readSensorData();
	printTestData();
}

